/*
 * Copyright (C) 2021-2022 NXP Semiconductors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <uwb_uwbs_tml_interface.h>
#include <uwb_bus_board.h>
#include "phUwbTypes.h"

#include "phOsalUwb.h"
#include "phNxpUwb_Common.h"
#include "phUwb_BuildConfig.h"
#include "phNxpLogApis_TmlUwb.h"
#include "phUwbErrorCodes.h"

#if UWBIOT_UWBD_SR2XXT

#if UWBIOT_OS_NATIVE
#include <unistd.h>
#endif

extern void AddDelayInMicroSec(int delay);

#define PAYLOAD_LEN_MSB 0x00
#define PAYLOAD_LEN_LSB 0x01
#define CRC_BYTES_LEN   0x02

#define UCI_RX_HDR_LEN             0x04
#define NORMAL_MODE_LEN_OFFSET     0x03
#define EXTND_LEN_INDICATOR_OFFSET 0x01
#define EXTENDED_LENGTH_OFFSET     0x02

#define DATA_TX_RX_PACKET_ID 0x02

#define UCI_HDR_LEN      0x04
#define HDLL_HEADER_SIZE 0x02

#define EXTND_LEN_INDICATOR_OFFSET_MASK 0x80
#define READ_IRQ_WAIT_TIME              (MAX_DELAY)

UWBStatus_t uwb_uwbs_tml_init(uwb_uwbs_tml_ctx_t *pCtx)
{
    UWBStatus_t status          = kUWBSTATUS_FAILED;
    uwb_bus_status_t bus_status = kUWB_bus_Status_FAILED;

    if (pCtx == NULL) {
        LOG_E("uwbs tml context is NULL");
        status = kUWBSTATUS_INVALID_PARAMETER;
        goto exit;
    }

    phOsalUwb_SetMemory(pCtx, 0, sizeof(uwb_uwbs_tml_ctx_t));

    // by default set tml mode to UCI
    pCtx->mode = kUWB_UWBS_TML_MODE_UCI;

    status = (UWBStatus_t)phOsalUwb_CreateMutex(&(pCtx->mSyncMutex));
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("Error: uwb_uwbs_tml_init(), could not create mutex mSyncMutex\n");
        goto exit;
    }

    bus_status = uwb_bus_init(&pCtx->busCtx);
    if (bus_status != kUWB_bus_Status_OK) {
        status = kUWBSTATUS_CONNECTION_FAILED;
        LOG_E("Error: uwb_uwbs_tml_init(), uwb bus initialisation failed");
        goto exit;
    }
    status = kUWBSTATUS_SUCCESS;
exit:
    return status;
}

UWBStatus_t uwb_uwbs_tml_setmode(uwb_uwbs_tml_ctx_t *pCtx, uwb_uwbs_tml_mode_t mode)
{
    if (pCtx == NULL) {
        LOG_E("uwbs tml context is NULL");
        return kUWBSTATUS_INVALID_PARAMETER;
    }
    pCtx->mode = mode;
    return kUWBSTATUS_SUCCESS;
}

UWBStatus_t uwb_uwbs_tml_deinit(uwb_uwbs_tml_ctx_t *pCtx)
{
    phOsalUwb_DeleteMutex(&(pCtx->mSyncMutex));
    uwb_bus_deinit(&pCtx->busCtx);
    phOsalUwb_SetMemory(pCtx, 0, sizeof(uwb_uwbs_tml_ctx_t));
    return kUWBSTATUS_SUCCESS;
}

UWBStatus_t uwb_uwbs_tml_data_tx(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t bufLen)
{
    uwb_bus_status_t bus_status = kUWB_bus_Status_FAILED;
    UWBStatus_t status          = kUWBSTATUS_FAILED;
    pCtx->noOfBytesWritten      = -1;

    if (pCtx == NULL) {
        LOG_E("uwbs tml context is NULL");
        status = kUWBSTATUS_INVALID_PARAMETER;
        goto end;
    }

    if (pBuf == NULL || bufLen == 0) {
        LOG_E("write buffer is Null or bufLen is 0");
        status = kUWBSTATUS_INVALID_PARAMETER;
        goto end;
    }

    phOsalUwb_LockMutex(pCtx->mSyncMutex);
    pCtx->busCtx.op_mode = WRITE_MODE;

    // pre write delay to allow the device to recover from the last batch of data
    // phOsalUwb_Delay(5);

    if (pCtx->mode == kUWB_UWBS_TML_MODE_HDLL) {
        bus_status = uwb_bus_data_trx(&pCtx->busCtx, pBuf, bufLen);
        if (bus_status == kUWB_bus_Status_FAILED) {
            LOG_E("uwb_bus_data_trx writing HDLL Command failed");
            goto end;
        }
    }
    else if (pCtx->mode == kUWB_UWBS_TML_MODE_UCI) {
        bus_status = uwb_bus_data_trx(&pCtx->busCtx, pBuf, bufLen);
        if (bus_status == kUWB_bus_Status_FAILED) {
            LOG_E("uwb_bus_data_trx writing UCI header failed");
            goto end;
        }

        // AddDelayInMicroSec(80);
        pCtx->noOfBytesWritten = bufLen;
    }
    else {
        LOG_E("uwb_bus_data_trx : tml mode not supported");
        goto end;
    }
    status = kUWBSTATUS_SUCCESS;
end:
    phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
    return status;
}

UWBStatus_t uwb_uwbs_tml_data_rx(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pBuf, size_t *pBufLen)
{
    uwb_bus_status_t bus_status = kUWB_bus_Status_FAILED;
    UWBStatus_t status          = kUWBSTATUS_FAILED;
    uint16_t payloadLen;
    uint16_t len                  = UCI_RX_HDR_LEN;
    uint16_t totalBtyesToRead     = 0;
    uint32_t IsExtndLenIndication = 0;

    if (pCtx == NULL) {
        LOG_E("uwb_uwbs_tml_data_rx : uwbs tml context is NULL");
        return kUWBSTATUS_INVALID_PARAMETER;
    }

    if (pBuf == NULL || pBufLen == NULL) {
        LOG_E("uwb_uwbs_tml_data_rx : read buffer is Null");
        return kUWBSTATUS_INVALID_PARAMETER;
    }

start:
    bus_status = uwb_bus_io_irq_wait(&pCtx->busCtx, READ_IRQ_WAIT_TIME);
    if (bus_status == kUWB_bus_Status_FAILED) {
        goto start;
    }
    /* If reader thread is waiting for irq and in between writer thread is proceeded(in PnP case)
    operation mode will be affected so, once irq is available set bus cntx to read mode */
    pCtx->busCtx.op_mode = READ_MODE;

    if (pCtx->mode == kUWB_UWBS_TML_MODE_HDLL) {
        if (*pBufLen == 0) {
            bus_status = uwb_bus_data_trx(&pCtx->busCtx, pBuf, HDLL_HEADER_SIZE);
            if (bus_status != kUWB_bus_Status_OK) {
                LOG_E("uwb_uwbs_tml_data_rx : reading from helios failed");
                *pBufLen = 0;
                goto exit;
            }
            payloadLen       = (uint16_t)(((pBuf[PAYLOAD_LEN_MSB] & 0x1F) << 8) | pBuf[PAYLOAD_LEN_LSB]);
            totalBtyesToRead = payloadLen + CRC_BYTES_LEN;
            if (payloadLen != 0) {
                bus_status = uwb_bus_data_trx(&pCtx->busCtx, &pBuf[HDLL_HEADER_SIZE], totalBtyesToRead);
                if (bus_status != kUWB_bus_Status_OK) {
                    LOG_E("uwb_uwbs_tml_data_rx : reading from helios failed");
                    *pBufLen = 0;
                    goto exit;
                }
            }
            *pBufLen = (uint8_t)(HDLL_HEADER_SIZE + payloadLen + CRC_BYTES_LEN);
        }
        else if (*pBufLen > 0) {
            bus_status = uwb_bus_data_trx(&pCtx->busCtx, pBuf, *pBufLen);
            if (bus_status != kUWB_bus_Status_OK) {
                LOG_E("uwb_uwbs_tml_data_rx : reading from helios failed");
                *pBufLen = 0;
                goto exit;
            }
        }
        else {
            return kUWBSTATUS_INVALID_PARAMETER;
        }
        status = kUWBSTATUS_SUCCESS;
    }
    else if (pCtx->mode == kUWB_UWBS_TML_MODE_UCI) {
        phOsalUwb_LockMutex(pCtx->mSyncMutex);
        *pBufLen = 0;
        // AddDelayInMicroSec(500);
        bus_status = uwb_bus_data_trx(&pCtx->busCtx, pBuf, len);
        if (bus_status == kUWB_bus_Status_OK) {
            *pBufLen = (size_t)(len);
        }
        else {
            LOG_E("uwb_uwbs_tml_data_rx : reading from helios failed");
            *pBufLen = 0;
            goto end;
        }

        if (pBuf[0] == DATA_TX_RX_PACKET_ID) {
            totalBtyesToRead = pBuf[NORMAL_MODE_LEN_OFFSET];
            totalBtyesToRead = (uint16_t)((totalBtyesToRead << 8) | pBuf[EXTENDED_LENGTH_OFFSET]);
        }
        else {
            IsExtndLenIndication = (pBuf[EXTND_LEN_INDICATOR_OFFSET] & EXTND_LEN_INDICATOR_OFFSET_MASK);

            totalBtyesToRead = pBuf[NORMAL_MODE_LEN_OFFSET];

            if (IsExtndLenIndication) {
                totalBtyesToRead = (uint16_t)((totalBtyesToRead << 8) | pBuf[EXTENDED_LENGTH_OFFSET]);
            }
        }

        payloadLen = totalBtyesToRead;

        if (payloadLen != 0) {
            bus_status = uwb_bus_data_trx(&pCtx->busCtx, &pBuf[len], payloadLen);
            if (bus_status == kUWB_bus_Status_OK) {
                *pBufLen = (uint16_t)(*pBufLen + payloadLen);
            }
            else {
                LOG_E("uwb_uwbs_tml_data_rx : reading from helios failed");
                *pBufLen = 0;
            }
        }

        // TODO: Disable the irq check to go low after read. It might possible that
        // irq goes high within some uSec if command contains some notification and MPU miss
        // to detect the IRQ state. eg. in one of the case irq pin toggle for sending ntf within 28usec.
#if 0
        uwbs_io_state_t gpioValue;
        uint8_t count                 = 0;
        AddDelayInMicroSec(1);
        uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
        while (gpioValue) {
            uwb_bus_io_val_get(&pCtx->busCtx, kUWBS_IO_I_UWBS_IRQ, &gpioValue);
            if (count >= 200) {
            	LOG_E("count: %d", count);
                goto end;
            }
            // Sleep of 500us * 20 = 10ms as per artf786394
            AddDelayInMicroSec(5);
            count++;
        }
#endif
        // LOG_MAU8_W("UCI RX < ", pBuf, *pBufLen);
        status = kUWBSTATUS_SUCCESS;
    end:
        phOsalUwb_UnlockMutex(pCtx->mSyncMutex);
    }
    else {
        LOG_E("uwb_uwbs_tml_data_tx : tml mode not supported");
    }

exit:
    AddDelayInMicroSec(50);
    return status;
}

UWBStatus_t uwb_uwbs_tml_data_trx(
    uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pTxBuf, size_t txBufLen, uint8_t *pRxBuf, size_t *pRxBufLen)
{
    UWBStatus_t status = kUWBSTATUS_FAILED;

    if (pCtx->mode == kUWB_UWBS_TML_MODE_HDLL) {
        // LOG_MAU8_I("HDLL Tx >", pTxBuf, txBufLen);
        *pRxBufLen = 0;
        status     = uwb_uwbs_tml_data_tx(pCtx, pTxBuf, txBufLen);
        if (status != kUWBSTATUS_SUCCESS) {
            LOG_E("uwb_uwbs_tml_hdll_data_trx write data failed");
            goto end;
        }
        status = uwb_uwbs_tml_data_rx(pCtx, pRxBuf, pRxBufLen);
        if (status != kUWBSTATUS_SUCCESS) {
            LOG_E("uwb_uwbs_tml_hdll_data_trx read data failed");
        }
        // LOG_MAU8_I("HDLL Rx <", pRxBuf, *pRxBufLen);
    end:
        AddDelayInMicroSec(50);
    }
    else {
        /* not needed for UCI Mode */
    }
    return status;
}

UWBStatus_t uwb_uwbs_tml_data_trx_with_Len(
    uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pTxBuf, size_t txBufLen, uint8_t *pRxBuf, size_t rxBufLen)
{
    UWBStatus_t status = kUWBSTATUS_FAILED;

    if (pRxBuf == NULL || rxBufLen == 0) {
        LOG_E("Response buffer is NULL or bytes to read is zero");
        return status;
    }

    if (pCtx->mode == kUWB_UWBS_TML_MODE_HDLL) {
        status = uwb_uwbs_tml_data_tx(pCtx, pTxBuf, txBufLen);
        if (status != kUWBSTATUS_SUCCESS) {
            LOG_E("uwb_uwbs_tml_HDLL_data_trx write data failed");
            goto end;
        }

        status = uwb_uwbs_tml_data_rx(pCtx, pRxBuf, &rxBufLen);
        if (status != kUWBSTATUS_SUCCESS) {
            LOG_E("uwb_uwbs_tml_HDLL_data_trx read data failed");
        }
        // LOG_MAU8_I("HDLL Rx >", pRxBuf , *pBufLen);

    end:
        AddDelayInMicroSec(50);
    }
    else {
        /* not needed for UCI Mode */
    }
    return status;
}

UWBStatus_t uwb_uwbs_tml_reset(uwb_uwbs_tml_ctx_t *pCtx)
{
    UWBStatus_t status = kUWBSTATUS_FAILED;
    uint8_t resp[20]   = {0};
    size_t resp_len    = 0;

    if (kUWB_bus_Status_OK != uwb_bus_reset(&pCtx->busCtx)) {
        return kUWBSTATUS_FAILED;
    }

    /* 1st Read HDLL Ready Notification */
    pCtx->mode = kUWB_UWBS_TML_MODE_HDLL;
    status     = uwb_uwbs_tml_data_rx(pCtx, resp, &resp_len);
    if (status != kUWBSTATUS_SUCCESS) {
        LOG_E("uwb_uwbs_tml_reset : uwb_uwbs_tml_data_rx failed");
        goto end;
    }

    // TODO: calculate crc and match with received one
    /* expected 0004810102007DC8 */
    if (resp_len == 0x08 && (*resp == 0x00) && (*(resp + 1) == 0x04) && (*(resp + 6) == 0x7D) &&
        (*(resp + 7) == 0xC8)) {
        /* Device is ready */
        status = kUWBSTATUS_SUCCESS;
    }
    else {
        status = kUWBSTATUS_FAILED;
    }
end:
    return status;
}

void uwb_uwbs_tml_flush_read_buffer(uwb_uwbs_tml_ctx_t *pCtx)
{
    /* Not needed here*/
    return;
}

UWBStatus_t uwb_uwbs_tml_helios_reset(uwb_uwbs_tml_ctx_t *pCtx, bool isFWDownloadDone)
{
    /* Not needed here */
    return kUWBSTATUS_SUCCESS;
}

UWBStatus_t uwb_uwbs_tml_helios_get_hdll_edl_ntf(uwb_uwbs_tml_ctx_t *pCtx, uint8_t *pRxBuf, size_t *pRxBufLen)
{
    /* Dummy function. Not needed here. */
    return UWBSTATUS_SUCCESS;
}

UWBStatus_t uwb_uwbs_tml_helios_hardreset(uwb_uwbs_tml_ctx_t *pCtx)
{
    /* Dummy function. Not needed here. */
    return UWBSTATUS_SUCCESS;
}

#endif // UWBIOT_UWBD_SR2XXT
